/*
 * HCSR04超声波模块引脚连接说明
 * | 功能名称      | 端口号    | 复用功能 | 板载引脚 |
 * |:-------------|:----------|:---------|:---------|
 * | VCC          | 5V        | -        | -        |
 * | GND          | GND       | -        | -        |
 * | TRIG         | PORTA31   | GPIO     | J10-8   |
 * | ECHO         | PORTA30   | GPIO     | J10-7  |
 */
#include "../board/sdk_project_config.h"
#include "peripherals_uart_2_config.h"
#include "pins_driver.h"
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "basic_api.h"

#define TRIG_PORT PORTA
#define TRIG_PIN 31U
#define ECHO_PORT PORTA
#define ECHO_PIN 30U

#define UART_TIMEOUT 500U
#define DELAY_US_PER_LOOP 10  // 先假设为10，后续校准

// 测距精度优化参数
#define MEASURE_COUNT 5        // 连续测量次数
#define MIN_DISTANCE_CM 2.0f   // 最小有效距离(cm)
#define MAX_DISTANCE_CM 400.0f // 最大有效距离(cm)
#define TEMPERATURE_C 25.0f    // 环境温度(摄氏度)，用于声速补偿

// 声速温度补偿函数 (cm/us)
float get_sound_speed(float temperature_c) {
    // 声速 = 331.4 + 0.6 * 温度(°C) cm/s
    // 转换为 cm/us
    return (331.4f + 0.6f * temperature_c) / 10000.0f;
}

// 单次测距函数
float single_measure_distance_cm(void) {
    uint32_t timeout = 60000; // 超时保护，单位：微秒

    // 0. 确保ECHO为低电平状态
    while (PINS_DRV_ReadPins(ECHO_PORT) & (1 << ECHO_PIN)) {
        BASIC_DelayUs(1);
    }

    // 1. 发送20us高电平脉冲到TRIG
    PINS_DRV_WritePin(TRIG_PORT, TRIG_PIN, 0);
    BASIC_DelayUs(2);
    PINS_DRV_WritePin(TRIG_PORT, TRIG_PIN, 1);
    BASIC_DelayUs(20); // 脉冲宽度加大
    PINS_DRV_WritePin(TRIG_PORT, TRIG_PIN, 0);

    // 2. 等待ECHO上升沿（增加短暂延时等待模块响应）
    BASIC_DelayUs(100); // 等待模块响应
    timeout = 60000;
    while (!(PINS_DRV_ReadPins(ECHO_PORT) & (1 << ECHO_PIN))) {
        if (--timeout == 0) {
            return -1.0f; // 超时
        }
        BASIC_DelayUs(1);
    }

    // 3. 计数ECHO高电平持续时间
    uint32_t echo_time_cnt = 0;
    timeout = 60000;
    while ((PINS_DRV_ReadPins(ECHO_PORT) & (1 << ECHO_PIN))) {
        if (--timeout == 0) return -2.0f; // 超时
        BASIC_DelayUs(1);
        echo_time_cnt++;
    }

    float real_echo_time_us = echo_time_cnt * DELAY_US_PER_LOOP;
    float sound_speed = get_sound_speed(TEMPERATURE_C);
    float distance = (real_echo_time_us / 2.0f) * sound_speed;
    
    return distance;
}

// 计算距离（单位：厘米）- 优化版本
float measure_distance_cm(void) {
    float distances[MEASURE_COUNT];
    float sum = 0.0f;
    int valid_count = 0;
    
    // 连续测量多次
    for (int i = 0; i < MEASURE_COUNT; i++) {
        distances[i] = single_measure_distance_cm();
        
        // 检查测量是否有效
        if (distances[i] > 0 && 
            distances[i] >= MIN_DISTANCE_CM && 
            distances[i] <= MAX_DISTANCE_CM) {
            sum += distances[i];
            valid_count++;
        }
        
        // 短暂延时避免干扰
        BASIC_DelayUs(1000);
    }
    
    // 如果没有有效测量，返回错误
    if (valid_count == 0) {
        return -3.0f; // 无有效测量
    }
    
    // 计算平均值
    float avg_distance = sum / valid_count;
    
    // 异常值过滤：如果某个测量值与平均值相差超过20%，则剔除
    float filtered_sum = 0.0f;
    int filtered_count = 0;
    for (int i = 0; i < MEASURE_COUNT; i++) {
        if (distances[i] > 0 && 
            distances[i] >= MIN_DISTANCE_CM && 
            distances[i] <= MAX_DISTANCE_CM) {
            float diff_ratio = fabs(distances[i] - avg_distance) / avg_distance;
            if (diff_ratio <= 0.2f) { // 允许20%的偏差
                filtered_sum += distances[i];
                filtered_count++;
            }
        }
    }
    
    // 如果过滤后没有有效值，使用原始平均值
    if (filtered_count == 0) {
        filtered_sum = sum;
        filtered_count = valid_count;
    }
    
    float final_distance = filtered_sum / filtered_count;
    
    // 调试信息
    char debug_buf[256];
    snprintf(debug_buf, sizeof(debug_buf),
        "[DEBUG] 测量次数:%d, 有效:%d, 过滤后:%d, 最终距离:%.1f cm\r\n",
        MEASURE_COUNT, valid_count, filtered_count, final_distance);
    UART_DRV_SendDataBlocking(INST_UART_2, (uint8_t*)debug_buf, strlen(debug_buf), UART_TIMEOUT);
    
    return final_distance;
}

int main(void)
{
    CLOCK_SYS_Init(g_pstClockManConfigsArr[0]);
    PINS_DRV_Init(NUM_OF_CONFIGURED_PINS, g_stPinmuxConfigArr);
    UART_DRV_Init(INST_UART_2, &g_stUartState_2, &g_stUart2UserConfig0);
    UART_DRV_SendDataBlocking(INST_UART_2, (uint8_t*)"HCSR04超声波测距演示\r\n", 28, UART_TIMEOUT);

    char uart_buf[64];
    while (1) {
        float distance = measure_distance_cm();
        if (distance == -1.0f) {
            snprintf(uart_buf, sizeof(uart_buf), "测距超时(上升沿)!\r\n");
        } else if (distance == -2.0f) {
            snprintf(uart_buf, sizeof(uart_buf), "测距超时(下降沿)!\r\n");
        } else if (distance == -3.0f) {
            snprintf(uart_buf, sizeof(uart_buf), "无有效测量!\r\n");
        } else {
            snprintf(uart_buf, sizeof(uart_buf), "距离: %.2f cm\r\n", distance);
        }
        UART_DRV_SendDataBlocking(INST_UART_2, (uint8_t*)uart_buf, strlen(uart_buf), UART_TIMEOUT);
        BASIC_DelayMs(500);
    }
}
